#include "fp.h"

const fp zero = {0, 0, 0, 0, 0, 0, 0, 0};
const fp one = {1, 0, 0, 0, 0, 0, 0, 0};
const fp sm2_p = {0xffffffff, 0xffffffff, 0x00000000, 0xffffffff,
                  0xffffffff, 0xffffffff, 0xffffffff, 0xfffffffe};
const fp sm2_n = {0x39d54123, 0x53bbf409, 0x21c6052b, 0x7203df6b,
                  0xffffffff, 0xffffffff, 0xffffffff, 0xfffffffe};

void fp_to_str(const fp n, str s) {
    static const char table[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
    u8 t[64];
    for (int i = 0; i < 8; i++)
        for (int j = 0; j < 8; j++)
            t[(i << 3) + j] = (n[i] >> (j >> 2)) & 0x0f;
    for (int i = 0; i < 64; ++i)
        s[63 - i] = table[t[i]];
}

void fp_fm_str(fp n, const str str) {
    static const u8 table[] = {
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 0, 0, 0, 0, 0,
            0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
            0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    u64 t[64];
    for (int i = 0; i < 64; i++) {
        t[63 - i] = table[str[i]];
    }
    for (int i = 0; i < 8; i++) {
        n[i] = 0;
        for (int j = 0; j < 8; j++)
            n[i] |= t[i * 8 + j] << (j * 4);
    }
}

void fp_to_byt(const fp n, byt b) {
    static const u32 pt[32] = {59, 58, 57, 56, 51, 50, 49, 48, 43, 42, 41, 40, 35, 34, 33, 32, 27, 26, 25, 24, 19, 18,
                               17, 16, 11, 10, 9, 8, 3, 2, 1, 0};
    u8 *t = (u8 *) n;
    for (int i = 0; i < 32; i++)
        b[i] = t[pt[i]];
}

void fp_fm_byt(fp n, const byt b) {
    static const u32 pt[32] = {59, 58, 57, 56, 51, 50, 49, 48, 43, 42, 41, 40, 35, 34, 33, 32, 27, 26, 25, 24, 19, 18,
                               17, 16, 11, 10, 9, 8, 3, 2, 1, 0};
    u8 *t = (u8 *) n;
    fp_set(n, zero);
    for (int i = 0; i < 32; i++)
        t[pt[i]] = b[i];
}

void fp_to_bit(const fp n, bit b) {
    for (int i = 255; i >= 0; i--)
        b[255 - i] = ((n[i / 32]) >> (i % 32)) & 1;
}

void fp_fm_bit(fp n, const bit b) {
    fp_set(n, zero);
    for (int i = 0; i < 256; i++)
        n[7 - (i >> 5)] |= (u64) b[i] << (31 - (i & 31));
}

void byt_to_str(const byt b, str s) {
    static const char table[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
    for (int i = 0, j = 0; i < 32; i++, j += 2) {
        s[j] = table[b[i] >> 4];
        s[j + 1] = table[b[i]];
    }
}

void byt_fm_str(byt b, const str s) {
    static const u8 table[] = {
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 0, 0, 0, 0, 0,
            0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
            0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    for (int j = 0; j < 64; j += 2)
        b[j >> 1] = table[s[j]] << 4 | table[s[j + 1]];
}

void fp_to_der(const fp n, u8 **dst, u64 *dlen) {
    byt b;
    fp_to_byt(n, b);
    asn1_integer_to_der(b, 32, dst, dlen);
}

void fp_fm_der(fp n, const u8 **src, u64 *slen) {
    u8 b[32];
    u64 l = 0;
    asn1_length_from_der(&l, src, NULL);
    // if (l > 32)
    //     error_print;
    // else
    if (l == 32) {
        asn1_integer_from_der(b, &l, src, slen);
    } else {
        asn1_integer_from_der(b, &l, src, slen);
        memmove(b + 32 - l, b, (u32) l);
        memset(b, 0, (u32) (32 - l));
    }
    fp_to_byt(n, b);
}

inline void add(fp r, const fp a, const fp b) {
    r[0] = a[0] + b[0];
    r[1] = a[1] + b[1] + (r[0] >> 32);
    r[2] = a[2] + b[2] + (r[1] >> 32);
    r[3] = a[3] + b[3] + (r[2] >> 32);
    r[4] = a[4] + b[4] + (r[3] >> 32);
    r[5] = a[5] + b[5] + (r[4] >> 32);
    r[6] = a[6] + b[6] + (r[5] >> 32);
    r[7] = a[7] + b[7] + (r[6] >> 32);
    r[0] &= 0xffffffff;
    r[1] &= 0xffffffff;
    r[2] &= 0xffffffff;
    r[3] &= 0xffffffff;
    r[4] &= 0xffffffff;
    r[5] &= 0xffffffff;
    r[6] &= 0xffffffff;
}

inline void sub(fp r, const fp a, const fp b) {
    r[0] = a[0] - b[0];
    r[1] = a[1] - b[1] - (r[0] >> 63);
    r[2] = a[2] - b[2] - (r[1] >> 63);
    r[3] = a[3] - b[3] - (r[2] >> 63);
    r[4] = a[4] - b[4] - (r[3] >> 63);
    r[5] = a[5] - b[5] - (r[4] >> 63);
    r[6] = a[6] - b[6] - (r[5] >> 63);
    r[7] = a[7] - b[7] - (r[6] >> 63);
    r[0] &= 0xffffffff;
    r[1] &= 0xffffffff;
    r[2] &= 0xffffffff;
    r[3] &= 0xffffffff;
    r[4] &= 0xffffffff;
    r[5] &= 0xffffffff;
    r[6] &= 0xffffffff;
}

inline void mul(dfp r, const fp a, const fp b){
    register u64 u, v = 0xFFFFFFFFULL;
    #pragma unroll
    for (int i = 0; i < 8; i++) {
        u = 0;
        for (int j = 0; j < 8; j++) {
            u += r[i + j] + a[i] * b[j];
            r[i + j] = u & v;
            u >>= 32;
        }
        r[i + 8] = u;
    }
}

inline void fp_set(fp r, const fp a) {
    r[0] = a[0];
    r[1] = a[1];
    r[2] = a[2];
    r[3] = a[3];
    r[4] = a[4];
    r[5] = a[5];
    r[6] = a[6];
    r[7] = a[7];
}

int fp_equ(const fp a, const fp b) {
    if (a[0] != b[0]) return 0;
    if (a[1] != b[1]) return 0;
    if (a[2] != b[2]) return 0;
    if (a[3] != b[3]) return 0;
    if (a[4] != b[4]) return 0;
    if (a[5] != b[5]) return 0;
    if (a[6] != b[6]) return 0;
    if (a[7] != b[7]) return 0;
    return 1;
}

int fp_cmp(const fp a, const fp b) {
    if (a[7] > b[7]) return 1;
    if (a[7] < b[7]) return 0;
    if (a[6] > b[6]) return 1;
    if (a[6] < b[6]) return 0;
    if (a[5] > b[5]) return 1;
    if (a[5] < b[5]) return 0;
    if (a[4] > b[4]) return 1;
    if (a[4] < b[4]) return 0;
    if (a[3] > b[3]) return 1;
    if (a[3] < b[3]) return 0;
    if (a[2] > b[2]) return 1;
    if (a[2] < b[2]) return 0;
    if (a[1] > b[1]) return 1;
    if (a[1] < b[1]) return 0;
    if (a[0] > b[0]) return 1;
    if (a[0] < b[0]) return 0;
    return 1;
}

int fp_tst(const fp a) {
    if (a[7] >> 63) return 0;
    if (a[7] > sm2_p[7]) return 0;
    if (a[7] < sm2_p[7]) return 1;
    if (a[6] > sm2_p[6]) return 0;
    if (a[6] < sm2_p[6]) return 1;
    if (a[5] > sm2_p[5]) return 0;
    if (a[5] < sm2_p[5]) return 1;
    if (a[4] > sm2_p[4]) return 0;
    if (a[4] < sm2_p[4]) return 1;
    if (a[3] > sm2_p[3]) return 0;
    if (a[3] < sm2_p[3]) return 1;
    if (a[2] > sm2_p[2]) return 0;
    if (a[2] < sm2_p[2]) return 1;
    if (a[1] > sm2_p[1]) return 0;
    if (a[1] < sm2_p[1]) return 1;
    if (a[0] > sm2_p[0]) return 0;
    if (a[0] < sm2_p[0]) return 1;
    return 0;
}

void fp_add(fp r, const fp a, const fp b) {
    add(r, a, b);
    if ((r[7] >> 32))
        sub(r, r, sm2_p);
}

void fp_sub(fp r, const fp a, const fp b) {
    sub(r, a, b);
    if (r[7] >> 63)
        add(r, r, sm2_p);
}

void fp_neg(fp r, const fp a) {
    r[0] = sm2_p[0] - a[0];
    r[1] = sm2_p[1] - a[1];
    r[2] = -a[2];
    r[3] = sm2_p[3] - a[3] - (r[2] >> 63);
    r[4] = sm2_p[4] - a[4] - (r[3] >> 63);
    r[5] = sm2_p[5] - a[5] - (r[4] >> 63);
    r[6] = sm2_p[6] - a[6] - (r[5] >> 63);
    r[7] = sm2_p[7] - a[7] - (r[6] >> 63);
    r[0] &= 0xffffffff;
    r[1] &= 0xffffffff;
    r[2] &= 0xffffffff;
    r[3] &= 0xffffffff;
    r[4] &= 0xffffffff;
    r[5] &= 0xffffffff;
    r[6] &= 0xffffffff;
}

void fp_haf(fp r, const fp a) {
    if (a[0] & 1) {
        add(r, a, sm2_p);
        r[0] = r[0] >> 1 | (u32) (r[1] << 31);
        r[1] = r[1] >> 1 | (u32) (r[2] << 31);
        r[2] = r[2] >> 1 | (u32) (r[3] << 31);
        r[3] = r[3] >> 1 | (u32) (r[4] << 31);
        r[4] = r[4] >> 1 | (u32) (r[5] << 31);
        r[5] = r[5] >> 1 | (u32) (r[6] << 31);
        r[6] = r[6] >> 1 | (u32) (r[7] << 31);
        r[7] = r[7] >> 1;
    } else {
        r[0] = a[0] >> 1 | (u32) (a[1] << 31);
        r[1] = a[1] >> 1 | (u32) (a[2] << 31);
        r[2] = a[2] >> 1 | (u32) (a[3] << 31);
        r[3] = a[3] >> 1 | (u32) (a[4] << 31);
        r[4] = a[4] >> 1 | (u32) (a[5] << 31);
        r[5] = a[5] >> 1 | (u32) (a[6] << 31);
        r[6] = a[6] >> 1 | (u32) (a[7] << 31);
        r[7] = a[7] >> 1;
    }
}

void fp_dou(fp r, const fp a) {
    r[7] = a[7] << 1 | a[6] >> 31;
    r[6] = (u32) (a[6] << 1) | a[5] >> 31;
    r[5] = (u32) (a[5] << 1) | a[4] >> 31;
    r[4] = (u32) (a[4] << 1) | a[3] >> 31;
    r[3] = (u32) (a[3] << 1) | a[2] >> 31;
    r[2] = (u32) (a[2] << 1) | a[1] >> 31;
    r[1] = (u32) (a[1] << 1) | a[0] >> 31;
    r[0] = (u32) (a[0] << 1);
    if ((r[7] >> 32))
        sub(r, r, sm2_p);
}

void fp_inv_loop(fp u, fp v, fp a, fp c) {
    while ((u[0] & 1) == 0) {
        u[0] = u[0] >> 1 | (u32) (u[1] << 31);
        u[1] = u[1] >> 1 | (u32) (u[2] << 31);
        u[2] = u[2] >> 1 | (u32) (u[3] << 31);
        u[3] = u[3] >> 1 | (u32) (u[4] << 31);
        u[4] = u[4] >> 1 | (u32) (u[5] << 31);
        u[5] = u[5] >> 1 | (u32) (u[6] << 31);
        u[6] = u[6] >> 1 | (u32) (u[7] << 31);
        u[7] = u[7] >> 1;
        if (a[0] & 1)
            add(a, a, sm2_p);
        a[0] = a[0] >> 1 | (u32) (a[1] << 31);
        a[1] = a[1] >> 1 | (u32) (a[2] << 31);
        a[2] = a[2] >> 1 | (u32) (a[3] << 31);
        a[3] = a[3] >> 1 | (u32) (a[4] << 31);
        a[4] = a[4] >> 1 | (u32) (a[5] << 31);
        a[5] = a[5] >> 1 | (u32) (a[6] << 31);
        a[6] = a[6] >> 1 | (u32) (a[7] << 31);
        a[7] = a[7] >> 1;
    }
    while ((v[0] & 1) == 0) {
        v[0] = v[0] >> 1 | (u32) (v[1] << 31);
        v[1] = v[1] >> 1 | (u32) (v[2] << 31);
        v[2] = v[2] >> 1 | (u32) (v[3] << 31);
        v[3] = v[3] >> 1 | (u32) (v[4] << 31);
        v[4] = v[4] >> 1 | (u32) (v[5] << 31);
        v[5] = v[5] >> 1 | (u32) (v[6] << 31);
        v[6] = v[6] >> 1 | (u32) (v[7] << 31);
        v[7] = v[7] >> 1;
        if (c[0] & 1)
            add(c, c, sm2_p);
        c[0] = c[0] >> 1 | (u32) (c[1] << 31);
        c[1] = c[1] >> 1 | (u32) (c[2] << 31);
        c[2] = c[2] >> 1 | (u32) (c[3] << 31);
        c[3] = c[3] >> 1 | (u32) (c[4] << 31);
        c[4] = c[4] >> 1 | (u32) (c[5] << 31);
        c[5] = c[5] >> 1 | (u32) (c[6] << 31);
        c[6] = c[6] >> 1 | (u32) (c[7] << 31);
        c[7] = c[7] >> 1;
    }
}

void fp_inv(fp r, const fp q) {
    fp u, v, a, c;
    fp_set(u, q);
    fp_set(v, sm2_p);
    fp_set(a, one);
    fp_set(c, zero);
    while (u[0]) {
        fp_inv_loop(u, v, a, c);
        if (fp_cmp(u, v)) {
            sub(u, u, v);
            fp_sub(a, a, c);
        } else {
            sub(v, v, u);
            fp_sub(c, c, a);
        }
    }
    fp_set(r, c);
}

void fp_mul(fp r, const fp a, const fp b) {
    // t = a * b
    dfp t = {0};
    mul(t, a, b);

    // t = t mod sm2_p
    fp_set(r, zero);
    u64 w[6] = {t[8] + t[9] + t[10] + t[11], t[8] + t[13], t[9] + t[14], t[14] + t[15], w[3] + t[13], w[4] + t[12]};
    w[0] += w[5];
    r[0] = t[0] + w[0] + w[4];
    r[1] += t[1] + w[0] + w[4] - w[1];
    r[2] += t[2] - w[1] - w[2];
    r[3] += t[3] + w[5] + w[1] + t[11];
    r[4] += t[4] + w[5] + w[2];
    r[5] += t[5] + w[4] + t[10] + t[15];
    r[6] += t[6] + w[3] + t[11];
    r[7] += t[7] + w[0] + w[5] + t[15];
    #pragma unroll
    for (int i = 1; i <= 7; i++) {
        r[i] += (int) (r[i - 1] >> 32);
        r[i - 1] &= 0xFFFFFFFFULL;
    }

    r[0] += (int) (r[7] >> 32);
    r[2] -= (int) (r[7] >> 32);
    r[3] += (int) (r[7] >> 32);
    r[7] += (int) (r[7] >> 32);
    #pragma unroll
    for (int i = 1; i <= 7; i++) {
        r[i] += (int) (r[i - 1] >> 32);
        r[i - 1] &= 0xFFFFFFFFULL;
    }
    r[7] &= 0xFFFFFFFFULL;
}

void fp_rand(fp r, const fp p) {
    fp_set(r, p);
}

void fn_add(fp r, const fp a, const fp b) {
    add(r, a, b);
    if ((r[7] >> 32))
        sub(r, r, sm2_n);
}

void fn_sub(fp r, const fp a, const fp b) {
    sub(r, a, b);
    if (r[7] >> 63)
        add(r, r, sm2_n);
}

void fn_inv_loop(fp u, fp v, fp a, fp c) {
    while ((u[0] & 1) == 0) {
        u[0] = u[0] >> 1 | (u32) (u[1] << 31);
        u[1] = u[1] >> 1 | (u32) (u[2] << 31);
        u[2] = u[2] >> 1 | (u32) (u[3] << 31);
        u[3] = u[3] >> 1 | (u32) (u[4] << 31);
        u[4] = u[4] >> 1 | (u32) (u[5] << 31);
        u[5] = u[5] >> 1 | (u32) (u[6] << 31);
        u[6] = u[6] >> 1 | (u32) (u[7] << 31);
        u[7] = u[7] >> 1;
        if (a[0] & 1)
            add(a, a, sm2_n);
        a[0] = a[0] >> 1 | (u32) (a[1] << 31);
        a[1] = a[1] >> 1 | (u32) (a[2] << 31);
        a[2] = a[2] >> 1 | (u32) (a[3] << 31);
        a[3] = a[3] >> 1 | (u32) (a[4] << 31);
        a[4] = a[4] >> 1 | (u32) (a[5] << 31);
        a[5] = a[5] >> 1 | (u32) (a[6] << 31);
        a[6] = a[6] >> 1 | (u32) (a[7] << 31);
        a[7] = a[7] >> 1;
    }
    while ((v[0] & 1) == 0) {
        v[0] = v[0] >> 1 | (u32) (v[1] << 31);
        v[1] = v[1] >> 1 | (u32) (v[2] << 31);
        v[2] = v[2] >> 1 | (u32) (v[3] << 31);
        v[3] = v[3] >> 1 | (u32) (v[4] << 31);
        v[4] = v[4] >> 1 | (u32) (v[5] << 31);
        v[5] = v[5] >> 1 | (u32) (v[6] << 31);
        v[6] = v[6] >> 1 | (u32) (v[7] << 31);
        v[7] = v[7] >> 1;
        if (c[0] & 1)
            add(c, c, sm2_n);
        c[0] = c[0] >> 1 | (u32) (c[1] << 31);
        c[1] = c[1] >> 1 | (u32) (c[2] << 31);
        c[2] = c[2] >> 1 | (u32) (c[3] << 31);
        c[3] = c[3] >> 1 | (u32) (c[4] << 31);
        c[4] = c[4] >> 1 | (u32) (c[5] << 31);
        c[5] = c[5] >> 1 | (u32) (c[6] << 31);
        c[6] = c[6] >> 1 | (u32) (c[7] << 31);
        c[7] = c[7] >> 1;
    }
}

void fn_inv(fp r, const fp q) {
    fp u, v, a, c;
    fp_set(u, q);
    fp_set(v, sm2_n);
    fp_set(a, one);
    fp_set(c, zero);
    while (u[0]) {
        fn_inv_loop(u, v, a, c);
        if (fp_cmp(u, v)) {
            sub(u, u, v);
            fn_sub(a, a, c);
        } else {
            sub(v, v, u);
            fn_sub(c, c, a);
        }
    }
    fp_set(r, c);
}

void fn_mul(fp r, const fp a, const fp b) {
    static const fp nu = {0xF15149A0, 0x12AC6361, 0xFA323C01, 0x8DFC2096,
                          0x00000001, 0x00000001, 0x00000001, 0x00000001};
    dfp x = {0}, y = {0}, z = {0};
    int i, k;
    // x = a * b, x' = x >> 256
    mul(x, a, b);

    // y = (x' * nu) >> 256 + x'  ( nu其实共257比特，其最高位为1，存储时省略了最高位， 此处 + x' 是补u的第257位的1)
    mul(y, x + 8, nu);
    add(y + 8, y + 8, x + 8);
    k = (int) (y[15] >> 32); //y可能为257比特，其最高位为1

    // z = y * n
    mul(z, y + 8, sm2_n);
    if (k)
        add(z + 8, z + 8, sm2_n);

    // x = x - z , 257位以上必然为0
    x[0] -= z[0];
    #pragma unroll
    for (i = 1; i < 9; i++) {
        x[i] = x[i] - z[i] - (x[i - 1] >> 63);
        x[i - 1] &= 0xFFFFFFFFULL;
    }

    // if x >= n, x = x - n
    if (x[8]) {
        x[7] |= 0x100000000;
        sub(x, x, sm2_n);
    }
    fp_set(r, x);
}

void fp_naf(naf r, u8 w, const fp n) {
    int i = 256;
    fp k, t;
    fp_set(k, n);
    while (fp_cmp(k, one)) {
        if (fp_is_odd(k)) {
            fp_set(t, zero);
            t[0] = k[0] & ((1 << w) - 1);       // t[0] = k[0] mod 2^w
            if (t[0] >> (w - 1)) {
                t[0] = (1 << w) - t[0];
                r[i] = (int) -t[0];
            } else {
                r[i] = (int) t[0];
            }
            if (r[i] > 0)
                sub(k, k, t);
            else
                add(k, k, t);
        } else
            r[i] = 0;
        fp_haf(k, k);
        i--;
    }
    while (i >= 0) {
        r[i--] = 0;
    }
}
